#include <physics/DistanceJoint.h>

#include <physics/World.h>
#include <physics/ValueLoader.h>
#include <Box2D/Dynamics/Joints/b2DistanceJoint.h>

namespace physics
{

DistanceJoint::DistanceJoint(World *world)
	: Joint(world)
{
	jointDef.reset(new b2DistanceJointDef());
}

DistanceJoint::DistanceJoint(World * world, Node * nodeA, Node  * nodeB, const Vector2 & anchorA, const Vector2 & anchorB, float length, float frequencyHz, bool collideConnected)
	: Joint(world)
{
	jointDef.reset(new b2DistanceJointDef());
	
	bodiesInfo.bodyA = nodeA;
	bodiesInfo.bodyB = nodeB;
	
	GetJointDef()->localAnchorA.x = anchorA.x / world->GetPixelsInMeter();
	GetJointDef()->localAnchorA.y = anchorA.y / world->GetPixelsInMeter();
	GetJointDef()->localAnchorB.x = anchorB.x / world->GetPixelsInMeter();
	GetJointDef()->localAnchorB.y = anchorB.y / world->GetPixelsInMeter();

	GetJointDef()->length = length / world->GetPixelsInMeter();

	GetJointDef()->frequencyHz = frequencyHz;
	
	GetJointDef()->collideConnected = collideConnected;
}

DistanceJoint::DistanceJoint(World * world, Node *nodeA, Node *nodeB, const Vector2 & anchorA, const Vector2 & anchorB, float length)
	: Joint(world)
{
	jointDef.reset(new b2DistanceJointDef());
	
	bodiesInfo.bodyA = nodeA;
	bodiesInfo.bodyB = nodeB;

	float pixInM = world->GetPixelsInMeter();

	GetJointDef()->Initialize(nodeA->GetBody(), nodeB->GetBody(), Converter::ToBox2D(anchorA / pixInM), Converter::ToBox2D(anchorB / pixInM));
	GetJointDef()->length = length / pixInM;
}

DistanceJoint::DistanceJoint(World * world, Node *nodeA, Node *nodeB, const Vector2 & anchorA, const Vector2 & anchorB, float minLength, float maxLength)
	: Joint(world)
{
	jointDef.reset(new b2DistanceJointDef());
	
	bodiesInfo.bodyA = nodeA;
	bodiesInfo.bodyB = nodeB;

	float pixInM = world->GetPixelsInMeter();

	GetJointDef()->Initialize(nodeA->GetBody(), nodeB->GetBody(), Converter::ToBox2D(anchorA / pixInM), Converter::ToBox2D(anchorB / pixInM));
	if(GetJointDef()->length < minLength / pixInM)
		GetJointDef()->length = minLength / pixInM;
	if(GetJointDef()->length > maxLength / pixInM)
		GetJointDef()->length = maxLength / pixInM;
}

DistanceJoint::DistanceJoint(World * world, Node *nodeA, Node *nodeB, const Vector2 & anchorA, const Vector2 & anchorB)
	: Joint(world)
{
	jointDef.reset(new b2DistanceJointDef());
	
	bodiesInfo.bodyA = nodeA;
	bodiesInfo.bodyB = nodeB;

	float pixInM = world->GetPixelsInMeter();

	GetJointDef()->Initialize(nodeA->GetBody(), nodeB->GetBody(), Converter::ToBox2D(anchorA / pixInM), Converter::ToBox2D(anchorB / pixInM));
}

DistanceJoint::~DistanceJoint()
{
}

bool DistanceJoint::on_node(xml::Provider *prov, const std::string &name)
{
	if(name == "Length")
	{
		if(!ValueReader::Read(prov, GetJointDef()->length))
			return false;
		
		GetJointDef()->length /= world->GetPixelsInMeter();
		return true;
	}
	if(name == "FrequencyHz")
		return ValueReader::Read(prov, GetJointDef()->frequencyHz);
	if(name == "DampingRatio")
		return ValueReader::Read(prov, GetJointDef()->dampingRatio);
	if(name == "LocalAnchorA" || name == "LocalAnchorB")
	{
		b2Vec2 &anchor = name == "LocalAnchorA" ? GetJointDef()->localAnchorA : GetJointDef()->localAnchorB;
		if(!ValueReader::Read(prov, anchor))
			return false;
		
		anchor.x /= world->GetPixelsInMeter();
		anchor.y /= world->GetPixelsInMeter();
		
		return true;
	}

	return Joint::on_node(prov, name);
}

}